#include "qbot_parser.h"
#include <iostream>
using namespace boost::placeholders;

void QbotWebParser::set_retcode(int msg_id, int code)
{
    try
    {
        ret_code.clear();
        ret_code["data"]["msgId"] = msg_id;
        ret_code["data"]["retCode"] = code;

        if (msg_id == ID_GET_ROBOT_STATE)
        {
            ret_code["data"]["robotState"] = this->robot_state;
        }
    }
    catch (const std::exception &e)
    {
        std::cerr << e.what() << '\n';
    }
}

QbotWebParser::QbotWebParser(QbotMaster* bot)
{
    this->robot = bot;
    this->init_time = boost::posix_time::second_clock::universal_time();
    this->ios_ = boost::make_shared<boost::asio::io_service>();
    this->msg_timer_ = boost::make_shared<boost::asio::deadline_timer>(ios_->get_executor().context(), boost::posix_time::millisec(100));
    msg_handler[ID_SET_VELOCITY] = boost::bind(&QbotWebParser::set_velocity, this, _1);
    msg_handler[ID_GET_ROBOT_STATE] = boost::bind(&QbotWebParser::get_robotState, this);
    msg_handler[ID_SET_POWER] = boost::bind(&QbotWebParser::set_power, this, _1);

    msg_timer_->async_wait(boost::bind(&QbotWebParser::msg_timer_handler, this));
    boost::thread( [&](){ ios_->run(); } );

}

void QbotWebParser::msg_timer_handler()
{
    client_timeout_cnt++;
    // std::cout << "timeout: " << (int)client_timeout_cnt << std::endl;
    msg_timer_->expires_at(msg_timer_->expires_at() + boost::posix_time::millisec(100));
    msg_timer_->async_wait(boost::bind(&QbotWebParser::msg_timer_handler, this));
}

bool QbotWebParser::parse(Json::Value &msg)
{
    bool ret = true;
    int msg_id = 0;
    set_retcode(0, -1);
    boost::mutex::scoped_lock lock(port_mutex);
    try
    {
        msg_id = msg["data"]["msgId"].asInt();
        if(msg_id<MAX_MESSAGE)
        {
            if(!msg_handler[msg_id].empty())
                msg_handler[msg_id](msg["data"]);
        }
    }
    catch(const std::exception& e)
    {
        std::cerr << e.what() << std::endl;
        ret = false;
    }
    
    return ret;
}

void QbotWebParser::set_velocity(Json::Value &msg)
{
    /*
    {
        "msgId" : 4,
        "move" : {
            "linear" : {
                "vx" : 1.0
            }
        }
    }
    */
    double vel = msg["move"]["linear"]["x"].asDouble();
    if(abs(vel)<=1.5)
    {
        robot->set_speed(vel);
        set_retcode(ID_SET_VELOCITY, 0);
    }
    else
    {
        set_retcode(ID_SET_VELOCITY, -1);
    }
    
}

void QbotWebParser::set_power(Json::Value &msg)
{
    /*
    {
        "msgId" : 4,
        "power" : {
            "onoff" : true
        }
    }
    */
    bool onoff = msg["power"]["onoff"].asBool();
    robot->set_power(onoff);
    set_retcode(ID_SET_POWER, 0);
}

void QbotWebParser::get_robotState()
{
    /*
    {
        "msgId" : 11,
        "robotState" : {
            "wheels" : [],
            "encoder" : {
                "turns" : 23.333
            }
        }
    }
    */
    static RobotState_T _state;
    static Json::Value _wheels[2];
    _state = robot->getState();
    running_time = (boost::posix_time::second_clock::universal_time() - init_time).total_seconds();
    robot_state["time"] = running_time;
    robot_state["encoder"]["turns"] = _state.encoder.turns;
    robot_state["encoder"]["ready"] = _state.encoder.ready;
    robot_state["encoder"]["odom"] = _state.encoder.odom;
    for(int i=0;i<2;i++){
        _wheels[i]["turns"] = _state.wheels[i].turns;
        _wheels[i]["ready"] = _state.wheels[i].ready;
        _wheels[i]["odom"] = _state.wheels[i].odom;
        _wheels[i]["actualRpm"] = _state.wheels[i].actual_rpm;
        robot_state["wheels"][i] = _wheels[i];
    }
    robot_state["BMS"]["RSOC"] = robot->battery_info.BMS.RSOC;
    client_timeout_cnt = 0; // clear timeout count
    set_retcode(ID_GET_ROBOT_STATE, 0);
}